using System;

using Kinova.API.Jaco;
using Kinova.DLL.SafeGate;
using Kinova.DLL.Data.Jaco;
using Kinova.DLL.Data.Jaco.Control;
using Kinova.DLL.Data.Jaco.Config;
using Kinova.DLL.Data.Jaco.Diagnostic;
using Kinova.DLL.Data.Util;
using Kinova.DLL.Tools.JacoStructures;
using Kinova.API.Jaco.IO.Comm.Exceptions;
using Kinova.API.Jaco.ValidationTools.Exceptions;
using Kinova.API.Jaco.Diagnostic;
using System.Xml;
using System.Xml.Linq;

using System.Net;
using System.Net.Sockets;
using System.Timers;
using System.IO;
using System.Diagnostics;


//########################################################################################################

namespace kinova_driver
{

	public class kinovaDriver : IDisposable
	{
//########################################################################################################		
		public kinovaDriver (string password, IPAddress ipAdress, int command_port)
		{
//			Console.TreatControlCAsInput = false;
//			Console.CancelKeyPress += new ConsoleCancelEventHandler(onCancelEvent);					
			mRun = true;
			kinova_password = password;
			destinationIpAddress = ipAdress;
			commandPort = command_port;
			resultEndPoint = new IPEndPoint( destinationIpAddress, commandPort );

		}
//########################################################################################################		
		public void Dispose ()
		{
			isExit = true;
			udpSocket.Disconnect(false);
			udpSocket =  null;
			
			try
			{
				System.Diagnostics.Trace.WriteLine("Destructing Kinova");	
				jacoArm.ControlManager.StopControlAPI();
				jacoArm.ControlManager.EraseTrajectories();
				jacoArm.CloseAll();
				
			}
			catch(Exception ex)
			{
				Console.WriteLine("Destructing Exception: "+ex.ToString() );
			}

			//this.Dispose();
			//throw new NotImplementedException ();
		}
		~kinovaDriver()
		{
		}
		public void checkIsReady()
		{
			Console.WriteLine("Retract Status "+jacoArm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().SystemStatus.RetractStatus);
			Console.WriteLine("Warning Low Voltage "+jacoArm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().SystemStatus.WarningLowVoltage);
			Console.WriteLine("Major Error Occured "+jacoArm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().SystemStatus.MajorErrorOccured);
			Console.WriteLine("Supply Voltage " + Convert.ToString(jacoArm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().SupplyVoltage));
			Console.WriteLine("Average Power "  + Convert.ToString(jacoArm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().AveragePower));
			Console.WriteLine("Supply Voltage " + Convert.ToString(jacoArm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().PowerConsumed));
			
			if( jacoArm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().SystemStatus.RetractStatus == 3 ||
		   		jacoArm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().SystemStatus.RetractStatus == 6 )
			{
				kinova_goto_homePos();
			}
			getStatus();
		}
//########################################################################################################
		public bool init ()
		{
			try
			{	
				CCypherMessage encryptedPassword = Crypto.GetInstance().Encrypt(kinova_password);

			    jacoArm = new CJacoArm(encryptedPassword);
				if(jacoArm.JacoIsReady())
				{
					jacoArm.ControlManager.StartControlAPI();
					Console.Clear();
					System.Console.WriteLine("\nInfo->JacoArm is Ready to use");	
					
					System.Console.WriteLine("Info->Reading Jaco's Client Congfiguration");	
					CClientConfigurations config = new CClientConfigurations();
					Console.WriteLine("Info-> API Version "+jacoArm.GetAPIVersion());
					config = jacoArm.ConfigurationsManager.GetClientConfigurations();
	
					System.Console.WriteLine("\tName : " + config.ClientName);
					System.Console.WriteLine("\tMax linear speed : " + config.MaxLinearSpeed); 
    				//config.MaxLinearSpeed = Kinova.DLL.Data.Jaco.CThreshold.LINEAR_SPEED_MAX_UPPER;
					
					//jacoArm.ConfigurationsManager.SetClientConfigurations(config);
					System.Threading.Thread.Sleep(1000);

					object sender = new object();
					System.Threading.ThreadPool.QueueUserWorkItem( new System.Threading.WaitCallback(receivePackage),sender);
					
				
						//Console.WriteLine("finger 2 "+jacoArm.ControlManager.GetForceAngularInfo().Finger2);
						//Console.WriteLine("finger 3 "+jacoArm.ControlManager.GetForceAngularInfo().Finger3);
								
					return true;
				}
				else
				{
					System.Console.WriteLine("\nFatal->JacoArm is not Ready to use");
				}
			}
			/*catch ( CAccessDeniedException ex)
            {
                System.Console.WriteLine("ACCESS DENIED - Password provided is invalid");
            }
            catch ( CJacoCommFailedException ex)
			{
				System.Console.WriteLine("Communication faild exception: "+ ex.Message);
			} */                      	
			catch(Exception ex)
			{
				System.Console.WriteLine("Error->Exception during execution of the driver. Verify " +
   									     		 "your API installation, verify if your Jaco is "   +
										 	     "connected and verify that " 						+
										         "you have a valid password:\n  "					+ ex.Message
										);
			}
			return false;
		}
//########################################################################################################
	public string getStatus()
    {
			try
			{
				Console.WriteLine("getStatus->getting current kinova status");
				currentCartesian_position = jacoArm.ControlManager.GetPositioningCartesianInfo();
				currentAngular_position   = jacoArm.ControlManager.GetPositioningAngularInfo();
			
				Console.WriteLine("cartesian pose.x: "+currentCartesian_position.X+
				                  "\ncartesian pose.y: "+currentCartesian_position.Y+
				                  "\ncartesian pose.z: "+currentCartesian_position.Z+
				                  "\ncartesian pose.ThetaX: "+currentCartesian_position.ThetaX+
				                  "\ncartesian pose.ThetaY: "+currentCartesian_position.ThetaY+
				                  "\ncartesian pose.ThetaZ: "+currentCartesian_position.ThetaZ+
				                  "\nJoint 1: "+currentAngular_position.Joint1+
				                  "\nJoint 2: "+currentAngular_position.Joint2+
				                  "\nJoint 3: "+currentAngular_position.Joint3+
				                  "\nJoint 4: "+currentAngular_position.Joint4+
				                  "\nJoint 4: "+currentAngular_position.Joint5+
				                  "\nJoint 5: "+currentAngular_position.Joint6);
				
				return generateStatusXml( currentCartesian_position, currentAngular_position).ToString();
			}
			catch(Exception ex)
			{	
				System.Console.WriteLine("Error-> getStatus: "+ex.Message);
			}
			return "";
    }
//########################################################################################################
	public XElement generateStatusXml( CCartesianInfo 	  cartesian_position,
									   CAngularInfo	      angular_position  )
	{
	XElement xml = new XElement("status",
			                            new XElement("cposition",
			                                         new XElement("position",
			                                         			new XElement("x",
			                                                                   cartesian_position.X
			                                            	    	           /*position.UserPosition.Position.Position[CVectorEuler.COORDINATE_X]*/),
			                                         			new XElement("y",
			                                                                   cartesian_position.Y
			                                            		               /*position.UserPosition.Position.Position[CVectorEuler.COORDINATE_Y]*/),
			                                         			new XElement("z",
			                                                                   cartesian_position.Z
			                                            		               /*position.UserPosition.Position.Position[CVectorEuler.COORDINATE_Z]*/) ),
			                                         new XElement("rotation",
			                                            		new XElement("theta_x",
			                                                                   cartesian_position.ThetaX
			                                                	          	   /*position.UserPosition.Position.Rotation[CVectorEuler.THETA_X]*/),
			                                   					new XElement("theta_y",
			                                                                   cartesian_position.ThetaY
			                                                      	   	  	   /*position.UserPosition.Position.Rotation[CVectorEuler.THETA_Y]*/),
			                                                    new XElement("theta_z",
			                                                                   cartesian_position.ThetaZ
			                                                                   /*position.UserPosition.Position.Rotation[CVectorEuler.THETA_Z]*/) ) ),
			                            new XElement("joints",
			                                         new XElement("joint_1",/*position.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_1]*/
			                                                      angular_position.Joint1),
			                                         new XElement("joint_2",/*position.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_2]*/
			                                                      angular_position.Joint2),
			                                         new XElement("joint_3",/*position.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_3]*/
			                                                      angular_position.Joint3),
			                                         new XElement("joint_4",/*position.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_4]*/
			                                                      angular_position.Joint4),
			                                         new XElement("joint_5",/*position.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_5]*/
			                                                      angular_position.Joint5),
			                                         new XElement("joint_6",/*position.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_6]*/
			                                                      angular_position.Joint6) ),
			                            new XElement("fingers",
			                                         new XElement("finger_1",/*position.UserPosition.FingerPosition[0]*/
			                                                      cartesian_position.Finger1),
			                                         new XElement("finger_2",/*position.UserPosition.FingerPosition[1]*/
			                                                      cartesian_position.Finger2),
			                                         new XElement("finger_3",
			                                                      cartesian_position.Finger3/*position.UserPosition.FingerPosition[2]*/))
			                                         );

			return xml;
			
	}
//########################################################################################################
	public void sendResult( string resultXml )
	{
			Socket udpSocket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp );
			udpSocket.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.Broadcast, true );
			udpSocket.SendTo( basicUtils.StrToByteArray(resultXml), resultEndPoint );
	}
	private volatile bool isExit = false;
	public  volatile bool isKill = false;
	Socket udpSocket = null;

		
//########################################################################################################
	public void receivePackage(object sender/*, ElapsedEventArgs e*/)
	{
		udpSocket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp); 
		IPEndPoint ipEndPoint = new IPEndPoint(IPAddress.Any, commandPort);
		udpSocket.Bind(ipEndPoint);
		Byte[] received = new Byte[2048];
		EndPoint remoteEP = (ipEndPoint);
					
		while(!isExit)
		{	
			try
			{
				int bytesReceived = udpSocket.ReceiveFrom(received, ref remoteEP);
				String dataReceived = System.Text.Encoding.ASCII.GetString(received);
				Console.WriteLine(dataReceived);
				String returningString = parsePackage(received);
				
				Byte[] returningByte = System.Text.Encoding.ASCII.GetBytes(returningString.ToCharArray());
				udpSocket.SendTo(returningByte, remoteEP);
		
			}
			catch (SocketException se)
			{
				Console.WriteLine("A Socket Exception has occurred!" + se.ToString());
			}
		}
	}
//########################################################################################################
	public string parsePackage( Byte[] byteArray )
	{
		MemoryStream stream = new MemoryStream(byteArray);		
		XmlReader xmlReader = XmlReader.Create(stream);
		CPosition cpos = new CPosition();
		CVectorAngle apos = new CVectorAngle();
		float[] fingersPosition = new float[3];
		isOperationTimeOuted = false;
		OPERATION_RESULT opResult;
		
		string resultString = "unknown";
			
		while( !xmlReader.IsStartElement() && !xmlReader.EOF)
		{
			xmlReader.Read();		
		}
		if(xmlReader.Name == "jaco" )
		{
			xmlReader.Read();
			if( xmlReader.Name == "status" )
			{
				Console.WriteLine("status request received");
				resultString = getStatus();			
			}
			else
			if( xmlReader.Name == "ping" )
			{
				Console.WriteLine("Ping request received");
				resultString = byteArray.Length+" bytes from client";
			}
			else
			if( xmlReader.Name == "gotopos" )
			{
				/*************************cartesian***************************/	
				if( xmlReader.GetAttribute("mode") == "cartesian" )
				{
				
					while( !(xmlReader.NodeType == XmlNodeType.EndElement &&
						   xmlReader.Name == "gotopos" ) )
						{
							if( xmlReader.Name == "position" )
							{
								while( !(xmlReader.NodeType == XmlNodeType.EndElement &&
						   				xmlReader.Name == "position" ) )
								{
									if( xmlReader.IsStartElement() )
									{
										if( xmlReader.Name == "x" )
										{
											cpos.UserPosition.Position.Position[CVectorEuler.COORDINATE_X] = 
												Convert.ToSingle(xmlReader.ReadString());	
										}
										if( xmlReader.Name == "y" )
										{
											cpos.UserPosition.Position.Position[CVectorEuler.COORDINATE_Y] = 
												Convert.ToSingle(xmlReader.ReadString());
										}
										if( xmlReader.Name == "z" )
										{
											cpos.UserPosition.Position.Position[CVectorEuler.COORDINATE_Z] = 
												Convert.ToSingle(xmlReader.ReadString());
										}
									}
									xmlReader.Read();
								}
							}
							if( xmlReader.Name == "rotation" )
							{
								while( !(xmlReader.NodeType == XmlNodeType.EndElement &&
						   				xmlReader.Name == "rotation" ) )
								{
									if( xmlReader.IsStartElement() )
									{
										if( xmlReader.Name == "x" )
										{
											cpos.UserPosition.Position.Rotation[CVectorEuler.THETA_X] = 
												Convert.ToSingle(xmlReader.ReadString());
											
										}
										if( xmlReader.Name == "y" )
										{
											cpos.UserPosition.Position.Rotation[CVectorEuler.THETA_Y] = 
												Convert.ToSingle(xmlReader.ReadString());
										}
										if( xmlReader.Name == "z" )
										{
											cpos.UserPosition.Position.Rotation[CVectorEuler.THETA_Z] = 
												Convert.ToSingle(xmlReader.ReadString());
										}
									}
									xmlReader.Read();
								}
							}
							xmlReader.Read();
						}
						if(kinova_goto_cpos(cpos))
						{
							opResult = kinovaDriver.OPERATION_RESULT.SUCCEEDED;
						}
						else
						{
							opResult = kinovaDriver.OPERATION_RESULT.FAILED;
						}
						resultString = generateOperationResultXml( "gotopos_cartesian", opResult).ToString();
				}
				/*************************Home***************************/
				else if( xmlReader.GetAttribute("mode") == "home" )
				{
					kinova_goto_homePos();
				}
				/*************************angular***************************/	
				else if(xmlReader.GetAttribute("mode") == "angular")
				{
					while( !(xmlReader.NodeType == XmlNodeType.EndElement &&
						   xmlReader.Name == "gotopos" ) )
						{
							if( xmlReader.Name == "joints" )
							{
								while( !(xmlReader.NodeType == XmlNodeType.EndElement &&
						   				xmlReader.Name == "joints" ) )
								{
									if( xmlReader.IsStartElement() )
									{
										if( xmlReader.Name == "joint_1" )
										{
											apos.Angle[CVectorAngle.JOINT_1] = Convert.ToSingle(xmlReader.ReadString());
										}
										if( xmlReader.Name == "joint_2" )
										{
											apos.Angle[CVectorAngle.JOINT_2] = Convert.ToSingle(xmlReader.ReadString());
										}
										if( xmlReader.Name == "joint_3" )
										{
											apos.Angle[CVectorAngle.JOINT_3] = Convert.ToSingle(xmlReader.ReadString());
										}
										if( xmlReader.Name == "joint_4" )
										{
											apos.Angle[CVectorAngle.JOINT_4] = Convert.ToSingle(xmlReader.ReadString());
										}
										if( xmlReader.Name == "joint_5" )
										{
											apos.Angle[CVectorAngle.JOINT_5] = Convert.ToSingle(xmlReader.ReadString());
										}
										if( xmlReader.Name == "joint_6" )
										{
											apos.Angle[CVectorAngle.JOINT_6] = Convert.ToSingle(xmlReader.ReadString());
										}
									}
									xmlReader.Read();
								}
							}
							xmlReader.Read();
						}
					if(kinova_goto_apos(apos))
						{
							opResult = kinovaDriver.OPERATION_RESULT.SUCCEEDED;
						}
						else
						{
							opResult = kinovaDriver.OPERATION_RESULT.FAILED;
						}
						resultString = generateOperationResultXml( "gotopos_angular", opResult).ToString();
				}
				/*******************************moveFingers****************************************/
				else if( xmlReader.GetAttribute("mode") == "fingers" )
				{
					while( !(xmlReader.NodeType == XmlNodeType.EndElement &&
						   xmlReader.Name == "gotopos" ) )
						{
							if( xmlReader.Name == "fingers" )
							{
								while( !(xmlReader.NodeType == XmlNodeType.EndElement &&
						   				xmlReader.Name == "fingers" ) )
								{
									if( xmlReader.IsStartElement() )
									{
										if( xmlReader.Name == "finger_1" )
										{
											fingersPosition[0] = Convert.ToSingle(xmlReader.ReadString());
										}
										if( xmlReader.Name == "finger_2" )
										{
											fingersPosition[1] = Convert.ToSingle(xmlReader.ReadString());
										}
										if( xmlReader.Name == "finger_3" )
										{
											fingersPosition[2] = Convert.ToSingle(xmlReader.ReadString());
										}
									}
									xmlReader.Read();
								}
							}
							xmlReader.Read();
						}
					if(kinova_setFingers(fingersPosition))
					{
							opResult = kinovaDriver.OPERATION_RESULT.SUCCEEDED;
					}
					else
					{
							opResult = kinovaDriver.OPERATION_RESULT.FAILED;
					}
					resultString = generateOperationResultXml( "gotopos_fingers", opResult).ToString();	
				}
			}
				/*TODO*/
			/*************************trajectory***************************/	
			else if( xmlReader.Name == "gotoTrajectories" )
			{
				if( xmlReader.GetAttribute("mode") == "cartesian" )
				{
					while( !(xmlReader.NodeType == XmlNodeType.EndElement &&
						   xmlReader.Name == "gotoTrajectories" ) )
						{
							xmlReader.Read();
						}	
				}	
			}				
		}
		return resultString;
	}		
//########################################################################################################
		public void kinova_goto_homePos()
		{
			try
			{
				Console.WriteLine("Moving to home pos");
				//mutex.WaitOne();
						jacoArm.ControlManager.StartControlAPI();
		        		//Default mapping for the API is that the third button(index = 2) control the retract/ready movement.
		        		CJoystickValue Value = new CJoystickValue();
		        while( jacoArm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().SystemStatus.RetractStatus != 1)
				{
						Value.ButtonValue[2] = 1;
		        		//Send a PRESS event on the third button(index 2).
						jacoArm.ControlManager.SendJoystickFunctionality(Value);
		        		//We wait 4 second to simulate a HOLD THE BUTTON FOR 4 SECONDS event
		        				
				//mutex.ReleaseMutex();
				}
        		Value.ButtonValue[2] = 0;
        		//Send a RELEASE event on the third button(index 2).
        		jacoArm.ControlManager.SendJoystickFunctionality(Value);
			}
			catch(Exception ex)
			{
				Console.WriteLine("Exception occurred: "+ex.ToString());				
			}/*
			catch(CJacoCommFailedException ex)
			{
				Console.WriteLine("CJacoCommFailedException:" + ex.Message);
			}
			catch(CKinovaCommBreakException ex)
			{
				Console.WriteLine("CKinovaCommBreakException:" + ex.Message);
			}
			catch(CKinovaCommDataCorruptException ex)
			{
				Console.WriteLine("CKinovaCommDataCorruptException:" + ex.Message);
			}
			catch(CKinovaCommTimeOutException ex)
			{
				Console.WriteLine("CKinovaCommTimeOutException:" + ex.Message);
			}*/
		}
		
		
//########################################################################################################
		public bool kinova_goto_cpos( CPosition cpos )
		{
			Console.WriteLine("MOVING TO CARTESIAN POS");
				CPointsTrajectory Trajectory 	 = new CPointsTrajectory();
				CTrajectoryInfo Point  			 = new CTrajectoryInfo();
				Point.UserPosition.PositionType = 
				Kinova.DLL.Data.Jaco.CJacoStructures.PositionType.CartesianPosition;
				
				cpos.UserPosition.FingerPosition[0] = 
				jacoArm.ControlManager.GetPositioningCartesianInfo().Finger1;
				cpos.UserPosition.FingerPosition[1] = 
				jacoArm.ControlManager.GetPositioningCartesianInfo().Finger2;
				cpos.UserPosition.FingerPosition[2] = 
				jacoArm.ControlManager.GetPositioningCartesianInfo().Finger3;
			
				Point.UserPosition = cpos.UserPosition;
				Point.UserPosition.HandMode = CJacoStructures.HandMode.PositionMode;
						
				try
				{
					mutex.WaitOne();
					jacoArm.ControlManager.StartControlAPI();
					jacoArm.ControlManager.EraseTrajectories();
					Trajectory.Add(Point);
					jacoArm.ControlManager.SendBasicTrajectory(Trajectory);
					mutex.ReleaseMutex();
				    
					Stopwatch timeout_sw = new Stopwatch();
				    timeout_sw.Start();
					
					do
					{
						try
						{
					    	if( (timeout_sw.ElapsedMilliseconds/1000) > CARTESIAN_MOVE_TIMEOUT )
							{
								mutex.WaitOne();
								jacoArm.ControlManager.EraseTrajectories();
								stopArm();
								Console.WriteLine("cartesian trajectory failed.");
							mutex.ReleaseMutex();
								return false;
							}
							mutex.WaitOne();
							currentCartesian_position = 
							jacoArm.ControlManager.GetPositioningCartesianInfo();
							mutex.ReleaseMutex();
						}
						catch(Exception ex)
						{
							Console.WriteLine("Exception: "+ex.Message);
							mutex.WaitOne();
							jacoArm.ReinitializeConnection();
							jacoArm.DiagnosticManager.DataManager.DeleteErrorLog();
							jacoArm.ControlManager.EraseTrajectories();
							mutex.ReleaseMutex();
							return false;
							
						}
					}    
					while( ( Math.Abs( cpos.UserPosition.Position.Position[CVectorEuler.COORDINATE_X] - 
				                	 currentCartesian_position.X) > CARTESIAN_TOLERANCE ) ||
				      	   ( Math.Abs( cpos.UserPosition.Position.Position[CVectorEuler.COORDINATE_Y] - 
				                	 currentCartesian_position.Y) > CARTESIAN_TOLERANCE ) ||
				      	   ( Math.Abs( cpos.UserPosition.Position.Position[CVectorEuler.COORDINATE_Z] - 
				                	 currentCartesian_position.Z) > CARTESIAN_TOLERANCE ) ||
				      	   ( Math.Abs( cpos.UserPosition.Position.Rotation[CVectorEuler.THETA_X]- 
				                	 currentCartesian_position.ThetaX) > CARTESIAN_ROTATION_TOLERANCE ) ||
				      	   ( Math.Abs( cpos.UserPosition.Position.Rotation[CVectorEuler.THETA_Y] - 
				                	 currentCartesian_position.ThetaY) > CARTESIAN_ROTATION_TOLERANCE ) || 
			             ( Math.Abs( cpos.UserPosition.Position.Rotation[CVectorEuler.THETA_Z] - 
				                	 currentCartesian_position.ThetaZ) > CARTESIAN_ROTATION_TOLERANCE ));
					Console.WriteLine("Info-> Reaching to Cartesian Position have been finished successfully");			
				}
			/*catch(CJacoCommFailedException ex)
			{
				Console.WriteLine("CJacoCommFailedException:" + ex.Message);
				return false;
			}
			catch(CKinovaCommBreakException ex)
			{
				Console.WriteLine("CKinovaCommBreakException:" + ex.Message);
				return false;
			}
			catch(CKinovaCommDataCorruptException ex)
			{
				Console.WriteLine("CKinovaCommDataCorruptException:" + ex.Message);
				return false;
			}
			catch(CKinovaCommTimeOutException ex)
			{
				Console.WriteLine("CKinovaCommTimeOutException:" + ex.Message);
				return false;
			}*/
			catch(Exception ex)
			{
				Console.WriteLine("Exception occurred : "+ex.ToString());
			}
			return true;
		}
//########################################################################################################
		public bool kinova_goto_apos(CVectorAngle apos)
		{
			    Console.WriteLine("MOVING TO ANGULAR POS");
				bool cancelTrajectory 			 = false;
				CPointsTrajectory Trajectory 	 = new CPointsTrajectory();
				CTrajectoryInfo Point  			 = new CTrajectoryInfo();
				Point.UserPosition.PositionType = 
				Kinova.DLL.Data.Jaco.CJacoStructures.PositionType.AngularPosition;
														
				Point.UserPosition.FingerPosition[0] = currentAngular_position.Finger1;
				Point.UserPosition.FingerPosition[1] = currentAngular_position.Finger2;
				Point.UserPosition.FingerPosition[2] = currentAngular_position.Finger3;
				
				Point.UserPosition.AnglesJoints = apos;
				Point.UserPosition.HandMode = CJacoStructures.HandMode.PositionMode;
				
				try
				{
					mutex.WaitOne();
					jacoArm.ControlManager.StartControlAPI();
					jacoArm.ControlManager.EraseTrajectories();
					Trajectory.Add(Point);
					jacoArm.ControlManager.SendTrajectoryFunctionnality(Trajectory);
					currentAngular_position = jacoArm.ControlManager.GetPositioningAngularInfo();
					mutex.ReleaseMutex();
					Stopwatch timeout_sw = new Stopwatch();
				    timeout_sw.Start();	
					
					do
					{
						try
						{
		
							if( (timeout_sw.ElapsedMilliseconds/1000) > ANGULAR_MOVE_TIMEOUT )
							{
								mutex.WaitOne();
								jacoArm.ControlManager.EraseTrajectories();
								Console.WriteLine("angular trajectory failed.");
								stopArm();	
								mutex.ReleaseMutex();
								return false;
							}
							currentAngular_position = 
							jacoArm.ControlManager.GetPositioningAngularInfo();
						}
						catch(Exception ex)
						{
							mutex.WaitOne();
							jacoArm.ReinitializeConnection();
							jacoArm.DiagnosticManager.DataManager.DeleteErrorLog();
							jacoArm.ControlManager.EraseTrajectories();
							mutex.ReleaseMutex();
							Console.WriteLine("Exception: "+ex.Message);
						}
					}
				while(  Math.Abs( ( currentAngular_position.Joint1 - apos.Angle[CVectorAngle.JOINT_1] ) ) > JOINT_TOLERANCE ||
			        	Math.Abs( ( currentAngular_position.Joint2 - apos.Angle[CVectorAngle.JOINT_2] ) ) > JOINT_TOLERANCE ||
			        	Math.Abs( ( currentAngular_position.Joint3 - apos.Angle[CVectorAngle.JOINT_3] ) ) > JOINT_TOLERANCE ||
			        	Math.Abs( ( currentAngular_position.Joint4 - apos.Angle[CVectorAngle.JOINT_4] ) ) > JOINT_TOLERANCE ||
			        	Math.Abs( ( currentAngular_position.Joint5 - apos.Angle[CVectorAngle.JOINT_5] ) ) > JOINT_TOLERANCE ||
			        	Math.Abs( ( currentAngular_position.Joint6 - apos.Angle[CVectorAngle.JOINT_6] ) ) > JOINT_TOLERANCE );
				

					Console.WriteLine("Info-> Reaching to Angular Position have been finished successfully");			
				}
			/*
				catch(CJacoCommFailedException ex)
				{
					Console.WriteLine("CJacoCommFailedException:" + ex.Message);
					return false;
				}
				catch(CKinovaCommBreakException ex)
				{
					Console.WriteLine("CKinovaCommBreakException:" + ex.Message);
					return false;
				}
				catch(CKinovaCommDataCorruptException ex)
				{
					Console.WriteLine("CKinovaCommDataCorruptException:" + ex.Message);
					return false;
				}
				catch(CKinovaCommTimeOutException ex)
				{
					Console.WriteLine("CKinovaCommTimeOutException:" + ex.Message);
					return false;	
				}*/
				catch(Exception ex)
				{
					Console.WriteLine("Exception occurred "+ex.ToString());		
				}
			return true;
				
		} 
//########################################################################################################		
	public bool kinova_setFingers( float[] fingersPosition )
	{
			Console.WriteLine("MOVING TO FINGERS POS");
				CPointsTrajectory Trajectory 	 = new CPointsTrajectory();				
			    CTrajectoryInfo Point 		     = new CTrajectoryInfo();
				Point.UserPosition.PositionType = 
				CJacoStructures.PositionType.CartesianPosition;	
				Point.UserPosition.FingerPosition[0] = fingersPosition[0];
				Point.UserPosition.FingerPosition[1] = fingersPosition[1];
				Point.UserPosition.FingerPosition[2] = fingersPosition[2];
				Point.UserPosition.HandMode = CJacoStructures.HandMode.PositionMode;
				//mutex.WaitOne();
				jacoArm.ControlManager.EraseTrajectories();
				Point.UserPosition.Position = jacoArm.ConfigurationsManager.GetHandPosition();
				/*Point.UserPosition.Position.Position[CVectorEuler.COORDINATE_X] = jacoArm.ControlManager.GetPositioningCartesianInfo().X;
				Point.UserPosition.Position.Position[CVectorEuler.COORDINATE_Y] = jacoArm.ControlManager.GetPositioningCartesianInfo().Y;
				Point.UserPosition.Position.Position[CVectorEuler.COORDINATE_Z] = jacoArm.ControlManager.GetPositioningCartesianInfo().Z;
				Point.UserPosition.Position.Rotation[CVectorEuler.THETA_X] = jacoArm.ControlManager.GetPositioningCartesianInfo().ThetaX;
				Point.UserPosition.Position.Rotation[CVectorEuler.THETA_Y] = jacoArm.ControlManager.GetPositioningCartesianInfo().ThetaY;
				Point.UserPosition.Position.Rotation[CVectorEuler.THETA_Z] = jacoArm.ControlManager.GetPositioningCartesianInfo().ThetaZ;
				*/
				Trajectory.Add(Point);
				try
				{
					jacoArm.ControlManager.StartControlAPI();		
					jacoArm.ControlManager.SendTrajectoryFunctionnality(Trajectory);
					Stopwatch timeout_sw = new Stopwatch();
				    timeout_sw.Start();	
					
					do
					{
						if( (timeout_sw.ElapsedMilliseconds/1000) > ANGULAR_MOVE_TIMEOUT )
						{
							mutex.WaitOne();
							jacoArm.ReinitializeConnection();
							jacoArm.DiagnosticManager.DataManager.DeleteErrorLog();
							jacoArm.ControlManager.EraseTrajectories();
							Console.WriteLine("setting fingers failed.");
							stopArm();
							mutex.ReleaseMutex();
							return false;
						}
						currentAngular_position = jacoArm.ControlManager.GetPositioningAngularInfo();
						
				}while(Math.Abs(currentAngular_position.Finger1 - fingersPosition[0]) > 15 ||
				       	   Math.Abs(currentAngular_position.Finger2 - fingersPosition[1]) > 15 ||
				       	   Math.Abs(currentAngular_position.Finger3 - fingersPosition[2]) > 15 );//adada avalesh 10 bood
					Console.WriteLine("Info-> Reaching to fingers Position have been finished successfully");			
				
				}
				/*
				catch(CJacoCommFailedException ex)
				{
					Console.WriteLine("CJacoCommFailedException:" + ex.Message);
				}
				catch(CKinovaCommBreakException ex)
				{
					Console.WriteLine("CKinovaCommBreakException:" + ex.Message);
				}
				catch(CKinovaCommDataCorruptException ex)
				{
					Console.WriteLine("CKinovaCommDataCorruptException:" + ex.Message);
				}
				catch(CKinovaCommTimeOutException ex)
				{
					Console.WriteLine("CKinovaCommTimeOutException:" + ex.Message);
				}*/
				catch(Exception ex)
				{
					Console.WriteLine(ex.ToString());
				}
				//mutex.ReleaseMutex();
				return true;
	}
//########################################################################################################
public XElement generateOperationResultXml( string operation, OPERATION_RESULT result)
{
			XElement xml = new XElement( "jaco",
			                             new XElement("type",operation),
			                             new XElement("result",result.ToString()) );
			return xml;                                         		
}
//########################################################################################################
public void stopArm()
{
			jacoArm.ControlManager.StopControlAPI();
			/*
			try
			{
				//mutex.WaitOne();
						jacoArm.ControlManager.StartControlAPI();
						jacoArm.ControlManager.EraseTrajectories();
						//Default mapping for the API is that the third button(index = 2) control the retract/ready movement.
		        		CJoystickValue Value = new CJoystickValue();
		        		Value.ButtonValue[2] = 1;
		        		//Send a PRESS event on the third button(index 2).
						jacoArm.ControlManager.SendJoystickFunctionality(Value);
		        		//We wait 4 second to simulate a HOLD THE BUTTON FOR 4 SECONDS event
		        		System.Threading.Thread.Sleep(10);
		        		Value.ButtonValue[2] = 0;
		        		//Send a RELEASE event on the third button(index 2).
		        		jacoArm.ControlManager.SendJoystickFunctionality(Value);		
				//mutex.ReleaseMutex();
			}
			catch(CJacoCommFailedException ex)
			{
				Console.WriteLine("CJacoCommFailedException:" + ex.Message);
			}
			catch(CKinovaCommBreakException ex)
			{
				Console.WriteLine("CKinovaCommBreakException:" + ex.Message);
			}
			catch(CKinovaCommDataCorruptException ex)
			{
				Console.WriteLine("CKinovaCommDataCorruptException:" + ex.Message);
			}
			catch(CKinovaCommTimeOutException ex)
			{
				Console.WriteLine("CKinovaCommTimeOutException:" + ex.Message);
			}
*/
}
//########################################################################################################
 protected void onCancelEvent(object sender, ConsoleCancelEventArgs args)
{
// Announce that the event handler has been invoked.
    Console.WriteLine("\nThe read operation has been interrupted.");

// Announce which key combination was pressed.
    Console.WriteLine("  Key pressed: {0}", args.SpecialKey);

// Announce the initial value of the Cancel property.
    Console.WriteLine("  Cancel property: {0}", args.Cancel);

// Set the Cancel property to true to prevent the process from terminating.
    Console.WriteLine("Setting the Cancel property to true...");
    args.Cancel = true;

// Announce the new value of the Cancel property.
    Console.WriteLine("  Cancel property: {0}", args.Cancel);
    Console.WriteLine("The read operation will resume...\n");
	isKill = true;	
}

		public const float JOINT_TOLERANCE = 1.0f; // 1.5 bood avalesh 		
		public const float CARTESIAN_TOLERANCE = 0.02f; // 0.05 bood avalesh
		public const float CARTESIAN_ROTATION_TOLERANCE = 0.01f;//0.02 bood 
		public const float CARTESIAN_MOVE_TIMEOUT = /*8*/8; //12 bood avalesh
		public const float ANGULAR_MOVE_TIMEOUT = /*8*/2; // 4 boodesh 
		
		private bool      isSendingCommand;
		private bool 	  mRun;
		private string 	  kinova_password;
		private CJacoArm  jacoArm;
		private CPosition currentPosition;
		CCartesianInfo 	  currentCartesian_position;
		CAngularInfo	  currentAngular_position;
		private IPAddress destinationIpAddress;
		private int 	  destinationPort;
		private int 	  commandPort;
		private IPEndPoint endPoint;
		private IPEndPoint resultEndPoint;
				
	
		private OPERATION_TYPE currentOperationType;
		private bool 		   isOperationTimeOuted;

		private  static System.Threading.Mutex mutex = new System.Threading.Mutex();

				
		
		
		private enum OPERATION_TYPE
		{
			GOTOPOS_CARTESIAN,
			GOTOPOS_ANGULAR,
			GOTOPOS_FINGERS
		};
		
		public enum OPERATION_RESULT
		{
			SUCCEEDED,
			FAILED
		};
	}
}
